#include <dispatch_system_ros_viz/moyingmap_viz.h>

using namespace moying::navigation;

namespace moying
{
namespace visualization
{
    MoyingMapViz::MoyingMapViz(const MoyingMapPtr &moyingmap, const std::string &global_frame)
    : nh_("MoyingMapViz"), moyingmap_(moyingmap), global_frame_(global_frame), timer_rate_(1)
    {
        costmap_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("costmap", 1, boost::bind(&MoyingMapViz::onNewCostmapSubscription, this, _1));
        raw_costmap_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("raw_costmap", 1, boost::bind(&MoyingMapViz::onNewRawCostmapSubscription, this, _1));
        timedmap_pub_ = nh_.advertise<visualization_msgs::Marker>("timedmap",10, true);
        costmap_coordinate_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("cost_map_coodinate",10, true);
        map_cell_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("map_cell",10, true);
        corrected_cell_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("corrected_cell",10, true);
        for(int i=0;i<sizeof(polygon_pub_)/sizeof(polygon_pub_[0]);i++){
            polygon_pub_[i] = nh_.advertise<geometry_msgs::PolygonStamped>("/collide_polygon"+std::to_string(i),10, true);
        }
        

        // pub_timer_ = nh_.createTimer(ros::Duration(0.5), &MoyingMapViz::timerUpdateCallback, this);
    }
    
    MoyingMapViz::~MoyingMapViz()
    {

    }

    void MoyingMapViz::prepareRawGrid(nav_msgs::OccupancyGrid &grid)
    {
        CostMapPtr costmap = moyingmap_->getCostMap();
        double resolution = costmap->getRawResolution();
        grid.header.frame_id = global_frame_;
        grid.header.stamp = ros::Time::now();
        grid.info.resolution = resolution;
        grid.info.width = costmap->getRawSizeInCellsX();
        grid.info.height = costmap->getRawSizeInCellsY();
        double wx, wy;
        costmap->rawMapToWorld(0, 0, wx, wy);
        grid.info.origin.position.x = wx - resolution / 2;
        grid.info.origin.position.y = wy - resolution / 2;
        grid.info.origin.position.z = 0.0;
        grid.info.origin.orientation.w = 1.0;
        std::vector<unsigned char> grid_data;
        costmap->getRaw1DCosts(grid_data);
        grid.data.resize(grid.info.width * grid.info.height);
        for (int i = 0; i < grid.data.size(); i++)
            grid.data[i] = grid_data[i];
    }

    void MoyingMapViz::prepareGrid(nav_msgs::OccupancyGrid &grid)
    {
        CostMapPtr costmap = moyingmap_->getCostMap();
        double resolution = costmap->getResolution();
        grid.header.frame_id = global_frame_;
        grid.header.stamp = ros::Time::now();
        grid.info.resolution = resolution;
        grid.info.width = costmap->getSizeInCellsX();
        grid.info.height = costmap->getSizeInCellsY();
        double wx, wy;
        costmap->mapToWorld(0, 0, wx, wy);
        grid.info.origin.position.x = wx - resolution / 2;
        grid.info.origin.position.y = wy - resolution / 2;
        grid.info.origin.position.z = 0.0;
        grid.info.origin.orientation.w = 1.0;
        std::vector<unsigned char> grid_data;
        costmap->get1DCosts(grid_data);
        grid.data.resize(grid.info.width * grid.info.height);
        for (int i = 0; i < grid.data.size(); i++)
            grid.data[i] = grid_data[i];
    }

    void MoyingMapViz::publishTimedMap(std::mutex& time_map_mutex,int time_long)
    {
        CostMapPtr costmap = moyingmap_->getCostMap();
        double resolution = costmap->getResolution();
        int sx = costmap->getSizeInCellsX();
        int sy = costmap->getSizeInCellsY();

        int st = moyingmap_->getTimeCounts();
        st = time_long > 0 && time_long < st ? time_long : st;


        // visualization_msgs::MarkerArray marker_array;
        // marker_array.markers.clear();

        // int i=0;
        // for(int x = 0; x < sx; x++)
        // {
        //     for(int y = 0; y < sy; y++)
        //     {   
        //         visualization_msgs::Marker marker;
        //         marker.header.frame_id = global_frame_;
        //         marker.header.stamp = ros::Time::now();
        //         marker.id = ++i;
        //         marker.action = visualization_msgs::Marker::ADD;
        //         marker.pose.orientation.w = 1.0;
        //         marker.scale.x = 0.9 * resolution;
        //         marker.scale.y = 0.9 * resolution;
        //         marker.scale.z = 0.02;
        //         marker.color.a = 1;
        //         marker.type = visualization_msgs::Marker::CUBE;

        //         auto time_list = (*moyingmap_->timed_map())[x][y];
        //         if(time_list == nullptr)
        //             continue;
        //         for(int t = 1; t < st; t++)
        //         {
        //             if(t >= time_list->size())
        //                 break;
        //             if(time_list->at(t) != nullptr)
        //             {
        //                 double wx, wy;
        //                 costmap->mapToWorld(x, y, wx, wy);
        //                 geometry_msgs::Point p;
        //                 p.x = wx;
        //                 p.y = wy;
        //                 p.z = 1 * t;
        //                 std_msgs::ColorRGBA c = getColorMsg(time_list->at(t));
        //                 marker.color = c;
        //                 marker.pose.position.x = p.x;
        //                 marker.pose.position.y = p.y;
        //                 marker.pose.position.z = p.z;
        //                 marker.pose.orientation.w = 1;
        //                 marker_array.markers.push_back(marker);
        //             }
        //         }
        //     }
        // }
        // timedmap_pub_.publish(marker_array);



        visualization_msgs::Marker marker;
        marker.header.frame_id = global_frame_;
        marker.header.stamp = ros::Time::now();
        marker.id = 0;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.orientation.w = 1.0;
        marker.scale.x = 0.9 * resolution;
        marker.scale.y = 0.9 * resolution;
        marker.scale.z = 0.02;
        marker.color.a = 1;
        marker.type = visualization_msgs::Marker::CUBE_LIST;
        for(int x = 0; x < sx; x++)
        {
            for(int y = 0; y < sy; y++)
            {   

                auto time_list = (*moyingmap_->timed_map())[x][y];
                if(time_list == nullptr)
                    continue;
                for(int t = 0; t < st; t++)
                {
                    if(t >= time_list->size())
                        break;

                    time_map_mutex.lock();
                    if(time_list->at(t) == nullptr){
                        time_map_mutex.unlock();
                        continue;
                    }else{
                        time_map_mutex.unlock();
                    }
                    double wx, wy;
                    costmap->mapToWorld(x, y, wx, wy);
                    geometry_msgs::Point p;
                    p.x = wx;
                    p.y = wy;
                    p.z = 1 * t;
                    std_msgs::ColorRGBA c = getColorMsg(time_list->at(t));
                    marker.colors.push_back(c);
                    marker.points.push_back(p);
                }
            }
        }

        // auto t = std::chrono::system_clock::now();
        timedmap_pub_.publish(marker);
// HIGHLIGHT("publish:"<<std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t).count());
    }

    void MoyingMapViz::onNewCostmapSubscription(const ros::SingleSubscriberPublisher& pub)
    {
        nav_msgs::OccupancyGrid grid;
        prepareGrid(grid);
        pub.publish(grid);
    }

    void MoyingMapViz::onNewRawCostmapSubscription(const ros::SingleSubscriberPublisher& pub)
    {
        nav_msgs::OccupancyGrid grid;
        prepareRawGrid(grid);
        pub.publish(grid);
    }

    void MoyingMapViz::timerUpdateCallback(const ros::TimerEvent& event)
    {
        // publishTimedMap();
        timer_rate_.sleep();
    }

    std_msgs::ColorRGBA MoyingMapViz::getColorMsg(const MapObjectPtr &object)
    {

        std_msgs::ColorRGBA msg;
        navigation::ColorRGBA color;
        if(object != nullptr && object.get() != nullptr)
            color = object->getColor();
        msg.r = color.r;
        msg.g = color.g;
        msg.b = color.b;
        msg.a = color.a;
        return msg;
    }

    void MoyingMapViz::publishCostmapCoordinate(){
        
        CostMapPtr costmap = moyingmap_->getCostMap();
        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();
        
        int w = costmap->getSizeInCellsX();
        int h = costmap->getSizeInCellsY();
        int i=0;
        for(int x = 0;x<w;x+=2){
            for(int y=0;y<h;y+=2){
                visualization_msgs::Marker marker;
                marker.header.frame_id = global_frame_;
                marker.header.stamp = ros::Time::now();
                marker.id = ++i;
                marker.action = visualization_msgs::Marker::ADD;
                costmap->mapToWorld(x, y, marker.pose.position.x, marker.pose.position.y);
                marker.pose.orientation.w = 1;
                marker.scale.x = 0.1;
                marker.scale.y = 0.1;
                marker.scale.z = 0.2;
                marker.color.g = 1;
                marker.color.a = 1;
                marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
                marker.text =  std::to_string(x) + "," + std::to_string(y);
                marker_array.markers.push_back(marker);
            }
        }

        costmap_coordinate_pub_.publish(marker_array);

    }

    void MoyingMapViz::publishMapCell(double length,double width){
        
        CostMapPtr costmap = moyingmap_->getCostMap();
        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();
        
        int w = costmap->getSizeInCellsX();
        int h = costmap->getSizeInCellsY();
        int i=0;
        auto cci = moyingmap_->corrected_cells(length,width);
        if(cci == nullptr){
            printf("!!!! publishMapCell failed, get corrected cell fail, len:%.2f width:%.2f\n",length,width);
            return;
        }
        for(int x = 0;x<w;x++){
            for(int y=0;y<h;y++){
                visualization_msgs::Marker marker;
                marker.header.frame_id = global_frame_;
                marker.header.stamp = ros::Time::now();
                marker.id = ++i;
                marker.action = visualization_msgs::Marker::ADD;
                costmap->mapToWorld(x, y, marker.pose.position.x, marker.pose.position.y);
                // printf("mx:%d my:%d wx:%.4f wy:%.4f\n",x,y,marker.pose.position.x, marker.pose.position.y);
                marker.pose.orientation.w = 1;
                float width = moyingmap_->getCostMap()->getResolution()-0.02;
                marker.scale.x = width;
                marker.scale.y = width;
                marker.scale.z = 0.01;

                unsigned int cost = (*cci)[x][y].cost;
                if(cost == 0 || cost == 255){
                    marker.color.b = 1;
                    marker.color.a = 0.2;
                }else if(cost < 255){
                    // printf("cost:%d\n",cost);
                    marker.color.r = 1.0/cost;
                    marker.color.a = 0.5;
                }
                
                marker.type = visualization_msgs::Marker::CUBE;
                marker_array.markers.push_back(marker);
            }
        }

        map_cell_pub_.publish(marker_array);

    }

    void MoyingMapViz::publishCorrectedCell(const std::vector<CorrectCellInfo>& cci_vec,float length,float width){
        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();
        
        int i=0;

        for(auto cci : cci_vec){
            if(cci.theta == 0)
                continue;
            visualization_msgs::Marker marker;
            marker.header.frame_id = global_frame_;
            marker.header.stamp = ros::Time::now();
            marker.id = ++i;
            marker.action = visualization_msgs::Marker::ADD;
            double x,y;
            moyingmap_->getCostMap()->rawMapToWorld(cci.raw_map_x, cci.raw_map_y, x, y);
            marker.pose.position.x = x;
            marker.pose.position.y = y;
            marker.pose.orientation.w = 1;
            marker.scale.x = 0.05;
            marker.scale.y = 0.05;
            marker.scale.z = 0.1;

            marker.color.r = 1;
            marker.color.a = 0.5;
            marker.type = visualization_msgs::Marker::CUBE;
            marker_array.markers.push_back(marker);

            // float theta = 0;
            // if(cci.theta <= 4 && cci.theta >= 0 ){
            //     theta = M_PI_4 * cci.theta;
            // }else if(cci.theta > 4 && cci.theta <= 7){
            //     theta = M_PI_4 * (cci.theta-8);
            // }
            // publishCollidePolygon(x,y,theta,length,width);
        }

        corrected_cell_pub_.publish(marker_array);  

    }


    void MoyingMapViz::publishCollidePolygon(float x,float y,float th,float length,float width){

        auto pub = [=](int rx,int ry,float theta,float l,float w,const ros::Publisher& publisher){

            // Coordinate lt,rt,rb,lb;
            // float center_wx = x;
            // float center_wy = y;
            // //机器人水平朝向的矩形坐标
            // lb.x = center_wx - length/2;  lb.y = center_wy - width/2;
            // rt.x = center_wx + length/2;  rt.y = center_wy + width/2;
            // lt.x = lb.x;    lt.y = rt.y;
            // rb.x = rt.x;    rb.y = lb.y;
            
    
            // //机器人旋转后的矩形坐标
            // x = lt.x; y = lt.y;
            // lt.x = (x-center_wx)*cos(theta) - (y-center_wy)*sin(theta) + center_wx;
            // lt.y = (x-center_wx)*sin(theta) + (y-center_wy)*cos(theta) + center_wy;

            // x = rt.x; y = rt.y;
            // rt.x = (x-center_wx)*cos(theta) - (y-center_wy)*sin(theta) + center_wx;
            // rt.y = (x-center_wx)*sin(theta) + (y-center_wy)*cos(theta) + center_wy;

            // x = rb.x; y = rb.y;
            // rb.x = (x-center_wx)*cos(theta) - (y-center_wy)*sin(theta) + center_wx;
            // rb.y = (x-center_wx)*sin(theta) + (y-center_wy)*cos(theta) + center_wy;

            // x = lb.x; y = lb.y;
            // lb.x = (x-center_wx)*cos(theta) - (y-center_wy)*sin(theta) + center_wx;
            // lb.y = (x-center_wx)*sin(theta) + (y-center_wy)*cos(theta) + center_wy;

        int width     = w / moyingmap_->getCostMap()->getRawResolution();
        int length    = l / moyingmap_->getCostMap()->getRawResolution();

        CoordinateInt lt,rt,rb,lb;
        //机器人水平朝向的矩形坐标
        lb.x = rx - length/2;  lb.y = ry - width/2;
        rt.x = rx + length/2;  rt.y = ry + width/2;
        lt.x = lb.x;    lt.y = rt.y;
        rb.x = rt.x;    rb.y = lb.y;

        //机器人旋转后的矩形坐标
        int x,y;
        x = lt.x; y = lt.y;
        lt.x = (x-rx)*cos(theta) - (y-ry)*sin(theta) + rx;
        lt.y = (x-rx)*sin(theta) + (y-ry)*cos(theta) + ry;

        x = rt.x; y = rt.y;
        rt.x = (x-rx)*cos(theta) - (y-ry)*sin(theta) + rx;
        rt.y = (x-rx)*sin(theta) + (y-ry)*cos(theta) + ry;

        x = rb.x; y = rb.y;
        rb.x = (x-rx)*cos(theta) - (y-ry)*sin(theta) + rx;
        rb.y = (x-rx)*sin(theta) + (y-ry)*cos(theta) + ry;

        x = lb.x; y = lb.y;
        lb.x = (x-rx)*cos(theta) - (y-ry)*sin(theta) + rx;
        lb.y = (x-rx)*sin(theta) + (y-ry)*cos(theta) + ry;


            geometry_msgs::PolygonStamped polygon;
            polygon.header.frame_id = global_frame_;
            polygon.header.stamp = ros::Time::now();
            double wx,wy;
            geometry_msgs::Point32 p;

            moyingmap_->getCostMap()->rawMapToWorld(lt.x,lt.y,wx,wy);
            p.x = wx;
            p.y = wy;
            polygon.polygon.points.push_back(p);
            moyingmap_->getCostMap()->rawMapToWorld(rt.x,rt.y,wx,wy);
            p.x = wx;
            p.y = wy;
            polygon.polygon.points.push_back(p);
            moyingmap_->getCostMap()->rawMapToWorld(rb.x,rb.y,wx,wy);
            p.x = wx;
            p.y = wy;
            polygon.polygon.points.push_back(p);
            moyingmap_->getCostMap()->rawMapToWorld(lb.x,lb.y,wx,wy);
            p.x = wx;
            p.y = wy;
            polygon.polygon.points.push_back(p);

            publisher.publish(polygon);
        };
        

        int rx,ry;
        moyingmap_->getCostMap()->worldToRawMap(x,y,rx,ry);
        for(int i=0;i<5;i++){
            pub(rx,ry,M_PI_4*i,length,width,polygon_pub_[i]);
        }

    }


}
}